#include "iic_8624.h"

#define IIC_DEVICE_ID		XPAR_XIICPS_0_DEVICE_ID
#define TEST_BUFFER_SIZE	132

XIicPs Iic; /**< Instance of the IIC Device */

u8 SendBuffer[TEST_BUFFER_SIZE]; /**< Buffer for Transmitting Data */
u8 RecvBuffer[TEST_BUFFER_SIZE]; /**< Buffer for Receiving Data */

int test_pex8624() {
	int Status;
	int Index;

	for (Index = 0; Index < TEST_BUFFER_SIZE; Index++) {
		SendBuffer[Index] = (Index % TEST_BUFFER_SIZE);
		RecvBuffer[Index] = 0;
	}

	xil_printf("IIC Master Polled Example Test \r\n");

	Status = iic_8624_init(&Iic, IIC_DEVICE_ID);
	if (Status != XST_SUCCESS) {
		xil_printf("8624 iic init failed!\r\n");
		return XST_FAILURE;
	}

	xil_printf("------------------------------\r\n");
	xil_printf("        PEX8624 Device ID     \r\n");
	xil_printf("------------------------------\r\n");

	SendBuffer[0] = 0x04;
	SendBuffer[1] = 0x04;
	SendBuffer[2] = 0x3c;
	SendBuffer[3] = 0x2a;   // device ID

	Status = iic_8624_read(&Iic, SendBuffer, RecvBuffer);
	if (Status != XST_SUCCESS) {
		xil_printf("8624 iic read failed!\r\n");
		return XST_FAILURE;
	}

	xil_printf("8624 iic read 0x%x !\r\n", SendBuffer[3] << 2);

	xil_printf("RecvBuffer[0] = 0x%02x\r\n", RecvBuffer[0]);
	xil_printf("RecvBuffer[1] = 0x%02x\r\n", RecvBuffer[1]);
	xil_printf("RecvBuffer[2] = 0x%02x\r\n", RecvBuffer[2]);
	xil_printf("RecvBuffer[3] = 0x%02x\r\n", RecvBuffer[3]);

	xil_printf("------------------------------\r\n");
	xil_printf("          FPGA port8 x8        \r\n");
	xil_printf("------------------------------\r\n");

	SendBuffer[0] = 0x04;
	SendBuffer[1] = 0x04;  // stn2 port8
	SendBuffer[2] = 0x3c;
	SendBuffer[3] = 0x1d;  //  0x74

	Status = iic_8624_read(&Iic, SendBuffer, RecvBuffer);
	if (Status != XST_SUCCESS) {
		xil_printf("8624 iic read failed!\r\n");
		return XST_FAILURE;
	}

	xil_printf("8624 iic read 0x%x !\r\n", SendBuffer[3] << 2);

	xil_printf("RecvBuffer[0] = 0x%02x\r\n", RecvBuffer[0]);
	xil_printf("RecvBuffer[1] = 0x%02x\r\n", RecvBuffer[1]);
	xil_printf("RecvBuffer[2] = 0x%02x\r\n", RecvBuffer[2]);
	xil_printf("RecvBuffer[3] = 0x%02x\r\n", RecvBuffer[3]);

	xil_printf("---------------------------\r\n");

	SendBuffer[0] = 0x04;
	SendBuffer[1] = 0x04;     // stn2 port8
	SendBuffer[2] = 0x3c;
	SendBuffer[3] = 0x1e;     // 0x78

	Status = iic_8624_read(&Iic, SendBuffer, RecvBuffer);
	if (Status != XST_SUCCESS) {
		xil_printf("8624 iic read failed!\r\n");
		return XST_FAILURE;
	}

	xil_printf("8624 iic read 0x%x !\r\n", SendBuffer[3] << 2);

	xil_printf("RecvBuffer[0] = 0x%02x\r\n", RecvBuffer[0]);
	xil_printf("RecvBuffer[1] = 0x%02x\r\n", RecvBuffer[1]);
	xil_printf("RecvBuffer[2] = 0x%02x\r\n", RecvBuffer[2]);
	xil_printf("RecvBuffer[3] = 0x%02x\r\n", RecvBuffer[3]);

	xil_printf("------------------------------\r\n");
	xil_printf("        CPU0 port6 x4         \r\n");
	xil_printf("------------------------------\r\n");

	SendBuffer[0] = 0x04;
	SendBuffer[1] = 0x03;   // stn1 port6
	SendBuffer[2] = 0x3c;
	SendBuffer[3] = 0x1d;  //  0x74

	Status = iic_8624_read(&Iic, SendBuffer, RecvBuffer);
	if (Status != XST_SUCCESS) {
		xil_printf("8624 iic read failed!\r\n");
		return XST_FAILURE;
	}

	xil_printf("8624 iic read 0x%x !\r\n", SendBuffer[3] << 2);

	xil_printf("RecvBuffer[0] = 0x%02x\r\n", RecvBuffer[0]);
	xil_printf("RecvBuffer[1] = 0x%02x\r\n", RecvBuffer[1]);
	xil_printf("RecvBuffer[2] = 0x%02x\r\n", RecvBuffer[2]);
	xil_printf("RecvBuffer[3] = 0x%02x\r\n", RecvBuffer[3]);

	xil_printf("---------------------------\r\n");

	SendBuffer[0] = 0x04;
	SendBuffer[1] = 0x03;
	SendBuffer[2] = 0x3c;
	SendBuffer[3] = 0x1e;     // 0x78

	Status = iic_8624_read(&Iic, SendBuffer, RecvBuffer);
	if (Status != XST_SUCCESS) {
		xil_printf("8624 iic read failed!\r\n");
		return XST_FAILURE;
	}

	xil_printf("8624 iic read 0x%x !\r\n", SendBuffer[3] << 2);

	xil_printf("RecvBuffer[0] = 0x%02x\r\n", RecvBuffer[0]);
	xil_printf("RecvBuffer[1] = 0x%02x\r\n", RecvBuffer[1]);
	xil_printf("RecvBuffer[2] = 0x%02x\r\n", RecvBuffer[2]);
	xil_printf("RecvBuffer[3] = 0x%02x\r\n", RecvBuffer[3]);

	xil_printf("------------------------------\r\n");
	xil_printf("        CPU1 port5 x4         \r\n");
	xil_printf("------------------------------\r\n");

	SendBuffer[0] = 0x04;
	SendBuffer[1] = 0x02;   // stn1 port5
	SendBuffer[2] = 0xbc;
	SendBuffer[3] = 0x1d;  //  0x74

	Status = iic_8624_read(&Iic, SendBuffer, RecvBuffer);
	if (Status != XST_SUCCESS) {
		xil_printf("8624 iic read failed!\r\n");
		return XST_FAILURE;
	}

	xil_printf("8624 iic read 0x%x !\r\n", SendBuffer[3] << 2);

	xil_printf("RecvBuffer[0] = 0x%02x\r\n", RecvBuffer[0]);
	xil_printf("RecvBuffer[1] = 0x%02x\r\n", RecvBuffer[1]);
	xil_printf("RecvBuffer[2] = 0x%02x\r\n", RecvBuffer[2]);
	xil_printf("RecvBuffer[3] = 0x%02x\r\n", RecvBuffer[3]);

	xil_printf("---------------------------\r\n");

	SendBuffer[0] = 0x04;
	SendBuffer[1] = 0x02;
	SendBuffer[2] = 0xbc;
	SendBuffer[3] = 0x1e;     // 0x78

	Status = iic_8624_read(&Iic, SendBuffer, RecvBuffer);
	if (Status != XST_SUCCESS) {
		xil_printf("8624 iic read failed!\r\n");
		return XST_FAILURE;
	}

	xil_printf("8624 iic read 0x%x !\r\n", SendBuffer[3] << 2);

	xil_printf("RecvBuffer[0] = 0x%02x\r\n", RecvBuffer[0]);
	xil_printf("RecvBuffer[1] = 0x%02x\r\n", RecvBuffer[1]);
	xil_printf("RecvBuffer[2] = 0x%02x\r\n", RecvBuffer[2]);
	xil_printf("RecvBuffer[3] = 0x%02x\r\n", RecvBuffer[3]);

	xil_printf("------------------------------\r\n");
	xil_printf("        CPU2 port0 x4         \r\n");
	xil_printf("------------------------------\r\n");

	SendBuffer[0] = 0x04;
	SendBuffer[1] = 0x0;  // stn0 port0
	SendBuffer[2] = 0x3c;
	SendBuffer[3] = 0x1d;  //  0x74

	Status = iic_8624_read(&Iic, SendBuffer, RecvBuffer);
	if (Status != XST_SUCCESS) {
		xil_printf("8624 iic read failed!\r\n");
		return XST_FAILURE;
	}

	xil_printf("8624 iic read 0x%x !\r\n", SendBuffer[3] << 2);

	xil_printf("RecvBuffer[0] = 0x%02x\r\n", RecvBuffer[0]);
	xil_printf("RecvBuffer[1] = 0x%02x\r\n", RecvBuffer[1]);
	xil_printf("RecvBuffer[2] = 0x%02x\r\n", RecvBuffer[2]);
	xil_printf("RecvBuffer[3] = 0x%02x\r\n", RecvBuffer[3]);

	xil_printf("---------------------------\r\n");

	SendBuffer[0] = 0x04;
	SendBuffer[1] = 0x0;
	SendBuffer[2] = 0x3c;
	SendBuffer[3] = 0x1e;     // 0x78

	Status = iic_8624_read(&Iic, SendBuffer, RecvBuffer);
	if (Status != XST_SUCCESS) {
		xil_printf("8624 iic read failed!\r\n");
		return XST_FAILURE;
	}

	xil_printf("8624 iic read 0x%x !\r\n", SendBuffer[3] << 2);

	xil_printf("RecvBuffer[0] = 0x%02x\r\n", RecvBuffer[0]);
	xil_printf("RecvBuffer[1] = 0x%02x\r\n", RecvBuffer[1]);
	xil_printf("RecvBuffer[2] = 0x%02x\r\n", RecvBuffer[2]);
	xil_printf("RecvBuffer[3] = 0x%02x\r\n", RecvBuffer[3]);

	xil_printf("------------------------------\r\n");
	xil_printf("        CPU3 port1 x4         \r\n");
	xil_printf("------------------------------\r\n");

	SendBuffer[0] = 0x04;
	SendBuffer[1] = 0x0;  // stn0 port0
	SendBuffer[2] = 0xbc;
	SendBuffer[3] = 0x1d;  //  0x74

	Status = iic_8624_read(&Iic, SendBuffer, RecvBuffer);
	if (Status != XST_SUCCESS) {
		xil_printf("8624 iic read failed!\r\n");
		return XST_FAILURE;
	}

	xil_printf("8624 iic read 0x%x !\r\n", SendBuffer[3] << 2);

	xil_printf("RecvBuffer[0] = 0x%02x\r\n", RecvBuffer[0]);
	xil_printf("RecvBuffer[1] = 0x%02x\r\n", RecvBuffer[1]);
	xil_printf("RecvBuffer[2] = 0x%02x\r\n", RecvBuffer[2]);
	xil_printf("RecvBuffer[3] = 0x%02x\r\n", RecvBuffer[3]);

	xil_printf("---------------------------\r\n");

	SendBuffer[0] = 0x04;
	SendBuffer[1] = 0x0;
	SendBuffer[2] = 0xbc;
	SendBuffer[3] = 0x1e;     // 0x78

	Status = iic_8624_read(&Iic, SendBuffer, RecvBuffer);
	if (Status != XST_SUCCESS) {
		xil_printf("8624 iic read failed!\r\n");
		return XST_FAILURE;
	}

	xil_printf("8624 iic read 0x%x !\r\n", SendBuffer[3] << 2);

	xil_printf("RecvBuffer[0] = 0x%02x\r\n", RecvBuffer[0]);
	xil_printf("RecvBuffer[1] = 0x%02x\r\n", RecvBuffer[1]);
	xil_printf("RecvBuffer[2] = 0x%02x\r\n", RecvBuffer[2]);
	xil_printf("RecvBuffer[3] = 0x%02x\r\n", RecvBuffer[3]);

	xil_printf("---------------------------\r\n");

	SendBuffer[0] = 0x04;
	SendBuffer[1] = 0x04;
	SendBuffer[2] = 0x3d;
	SendBuffer[3] = 0x9a;     // 0x668

	Status = iic_8624_read(&Iic, SendBuffer, RecvBuffer);
	if (Status != XST_SUCCESS) {
		xil_printf("8624 iic read failed!\r\n");
		return XST_FAILURE;
	}

	xil_printf("8624 iic read 0x%x !\r\n", (SendBuffer[3] << 2) + 0x400);

	xil_printf("RecvBuffer[0] = 0x%02x\r\n", RecvBuffer[0]);
	xil_printf("RecvBuffer[1] = 0x%02x\r\n", RecvBuffer[1]);
	xil_printf("RecvBuffer[2] = 0x%02x\r\n", RecvBuffer[2]);
	xil_printf("RecvBuffer[3] = 0x%02x\r\n", RecvBuffer[3]);

	xil_printf("---------------------------\r\n");

	SendBuffer[0] = 0x04;
	SendBuffer[1] = 0x04;
	SendBuffer[2] = 0x3c;
	SendBuffer[3] = 0x93;     // 0x24c

	Status = iic_8624_read(&Iic, SendBuffer, RecvBuffer);
	if (Status != XST_SUCCESS) {
		xil_printf("8624 iic read failed!\r\n");
		return XST_FAILURE;
	}

	xil_printf("8624 iic read 0x%x !\r\n", SendBuffer[3] << 2);

	xil_printf("RecvBuffer[0] = 0x%02x\r\n", RecvBuffer[0]);
	xil_printf("RecvBuffer[1] = 0x%02x\r\n", RecvBuffer[1]);
	xil_printf("RecvBuffer[2] = 0x%02x\r\n", RecvBuffer[2]);
	xil_printf("RecvBuffer[3] = 0x%02x\r\n", RecvBuffer[3]);

	xil_printf("---------------------------\r\n");


	SendBuffer[0] = 0x03;
	SendBuffer[1] = 0x04;
	SendBuffer[2] = 0x3c;
	SendBuffer[3] = 0x14;
	SendBuffer[4] = 0x12;
	SendBuffer[5] = 0x34;
	SendBuffer[6] = 0x56;
	SendBuffer[7] = 0x78;

	Status = iic_8624_write(&Iic, SendBuffer);

	SendBuffer[0] = 0x04;
	SendBuffer[1] = 0x04;
	SendBuffer[2] = 0x3c;
	SendBuffer[3] = 0x14;

	Status = iic_8624_read(&Iic, SendBuffer, RecvBuffer);
	if (Status != XST_SUCCESS) {
		xil_printf("8624 iic read failed!\r\n");
		return XST_FAILURE;
	}

	xil_printf("RecvBuffer[0] = 0x%02x\r\n", RecvBuffer[0]);
	xil_printf("RecvBuffer[1] = 0x%02x\r\n", RecvBuffer[1]);
	xil_printf("RecvBuffer[2] = 0x%02x\r\n", RecvBuffer[2]);
	xil_printf("RecvBuffer[3] = 0x%02x\r\n", RecvBuffer[3]);

	return XST_SUCCESS;
}

#if 0
int IicPsMasterPolledExample(u16 DeviceId) {
	int Status;
	XIicPs_Config *Config;
	int Index;

	/*
	 * Initialize the IIC driver so that it's ready to use
	 * Look up the configuration in the config table,
	 * then initialize it.
	 */
	Config = XIicPs_LookupConfig(DeviceId);
	if (NULL == Config) {
		return XST_FAILURE;
	}

	Status = XIicPs_CfgInitialize(&Iic, Config, Config->BaseAddress);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Perform a self-test to ensure that the hardware was built correctly.
	 */
	Status = XIicPs_SelfTest(&Iic);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Set the IIC serial clock rate.
	 */
	XIicPs_SetSClk(&Iic, IIC_SCLK_RATE);

	/*
	 * Initialize the send buffer bytes with a pattern to send and the
	 * the receive buffer bytes to zero to allow the receive data to be
	 * verified.
	 */
	for (Index = 0; Index < TEST_BUFFER_SIZE; Index++) {
		SendBuffer[Index] = (Index % TEST_BUFFER_SIZE);
		RecvBuffer[Index] = 0;
	}

	SendBuffer[0] = 0x04;
	SendBuffer[1] = 0x04;
	SendBuffer[2] = 0x3c;
	SendBuffer[3] = 0x2a;

	/*
	 * Send the buffer using the IIC and ignore the number of bytes sent
	 * as the return value since we are using it in interrupt mode.
	 */
	//Status = XIicPs_MasterSendPolled(&Iic, SendBuffer,
	//		 TEST_BUFFER_SIZE, IIC_SLAVE_ADDR);
	Status = XIicPs_MasterSendPolled(&Iic, SendBuffer, 4, IIC_SLAVE_ADDR);

	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

//	delay_time=30;   /* seconds */

	/*	for (i = 0;i < delay_time; i++) {
	 for (k = 0; k < (125000000*8)/56; k++) {
	 asm("nop");
	 }*/
//	delay_SL(3000000);
	/*
	 * Wait until bus is idle to start another transfer.
	 */
	while (XIicPs_BusIsBusy(&Iic)) {
		/* NOP */
		//	asm("nop");
		//	asm("nop");
		//	asm("nop");
	}

	Status = XIicPs_MasterRecvPolled(&Iic, RecvBuffer, 4, IIC_SLAVE_ADDR);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	xil_printf("RecvBuffer[0] = 0x%02x\r\n", RecvBuffer[0]);
	xil_printf("RecvBuffer[1] = 0x%02x\r\n", RecvBuffer[1]);
	xil_printf("RecvBuffer[2] = 0x%02x\r\n", RecvBuffer[2]);
	xil_printf("RecvBuffer[3] = 0x%02x\r\n", RecvBuffer[3]);

	SendBuffer[0] = 0x03;
	SendBuffer[1] = 0x04;
	SendBuffer[2] = 0x3c;
	SendBuffer[3] = 0x14;
	SendBuffer[4] = 0x12;
	SendBuffer[5] = 0x34;
	SendBuffer[6] = 0x56;
	SendBuffer[7] = 0x78;

	Status = XIicPs_MasterSendPolled(&Iic, SendBuffer, 8, IIC_SLAVE_ADDR);

	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	while (XIicPs_BusIsBusy(&Iic)) {
		/* NOP */
		//	asm("nop");
		//	asm("nop");
		//	asm("nop");
	}

	SendBuffer[0] = 0x03;
	SendBuffer[1] = 0x04;
	SendBuffer[2] = 0x3c;
	SendBuffer[3] = 0x14;

	Status = XIicPs_MasterSendPolled(&Iic, SendBuffer, 4, IIC_SLAVE_ADDR);

	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	while (XIicPs_BusIsBusy(&Iic)) {
		/* NOP */
		//	asm("nop");
		//	asm("nop");
		//	asm("nop");
	}

	Status = XIicPs_MasterRecvPolled(&Iic, RecvBuffer, 4, IIC_SLAVE_ADDR);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	xil_printf("read RecvBuffer[0] = 0x%02x\r\n", RecvBuffer[0]);
	xil_printf("read RecvBuffer[1] = 0x%02x\r\n", RecvBuffer[1]);
	xil_printf("read RecvBuffer[2] = 0x%02x\r\n", RecvBuffer[2]);
	xil_printf("read RecvBuffer[3] = 0x%02x\r\n", RecvBuffer[3]);

	return 0;
	/*
	 * Verify received data is correct.
	 */
	for (Index = 0; Index < TEST_BUFFER_SIZE; Index++) {

		/* Aardvark as slave can only set 64 bytes for output */
		if (RecvBuffer[Index] != Index % 64) {
			return XST_FAILURE;
		}
	}

	return XST_SUCCESS;
}
#endif
